﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Activities;
using System.ComponentModel;
using System.Activities.Presentation;
using Wilke.Interactive.Drone.Control.Enumerations;
using Wilke.Interactive.Drone.Contracts;
using System.Diagnostics;
using System.Threading;
using System.Drawing;

namespace Wilke.Interactive.Drone.Extensions.Activities
{
    [Designer(typeof(PilotDesigner))]
    [ToolboxBitmap(typeof(PathDesigner), "Resources.Icons.Pilot.png")]
    public class PilotActivity : TimedActivity
    {
        #region Private Properties

        bool doContinue = false;
        private System.Timers.Timer timer;
        private float compensationFactor = 1.4f;

        #endregion

        #region Public Properties

        public float Roll { get; set; }
        public float Pitch { get; set; }
        public float Yaw{ get; set; }
        public float Height { get; set; }
        public bool Compensate { get; set; }

        #endregion

        protected override void Execute(CodeActivityContext context)
        {
            base.Execute(context);
            if (DroneCommander != null)
            {
                timer = new System.Timers.Timer(Duration * 1000);
                timer.Elapsed += new System.Timers.ElapsedEventHandler(timer_Elapsed);
                timer.Start();

                doContinue = true;

                while(doContinue)
                {
                    DroneCommander.SetFlightParameters(Roll / 100, - Pitch / 100, Height / 100, Yaw / 100);
                    Thread.Sleep(25);
                }

                if (Compensate)
                {
                    #region Experimental - Compensate for drift (needs finetuning)

                    timer = new System.Timers.Timer(250);
                    timer.Elapsed += new System.Timers.ElapsedEventHandler(timer_Elapsed);
                    timer.Start();

                    doContinue = true;

                    while (doContinue)
                    {
                        DroneCommander.SetFlightParameters((-Roll / 100) * compensationFactor, (Pitch / 100) * compensationFactor, 0, (-Yaw / 100) * compensationFactor);
                        Thread.Sleep(25);
                    }

                    #endregion
                }

                DroneCommander.SetFlightParameters(0, 0, 0, 0);
             }
            else
            {
                throw new Exception("Could not retrieve an active instance of 'DroneCommander'");
            }
        }

        void timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            timer.Stop();
            doContinue = false;
        }
    }
}
